#include <iostream>
#include "read_data.h"
#include "imu_integration.h"
#include "optimizer.h"
using namespace std;

int main() {  
	double acc_noise_std = 0.1;
	double init_bias = 0.0;
	std::vector<double> timestamps;
	std::vector<Eigen::Vector3d> imu_data;  
	std::vector<Eigen::Vector3d> odometry_data;
	std::vector<Eigen::Vector3d> vel_data;  
	std::vector<Eigen::Vector3d> pose_data;
	std::vector<Eigen::Vector3d> gps_data;
	read_simulate(init_bias, timestamps, imu_data, odometry_data, vel_data, pose_data, gps_data);
	
	ImuIntegration imu_integration(init_bias);
    State state; 
    memset(&state, 0.0, sizeof(state));
    std::vector<Eigen::Vector3d> pose_gt = pose_data;
    double linear_vel = 0.5;
    double true_vx = linear_vel * std::cos(pose_gt[0][2]);
    double true_vy = linear_vel * std::sin(pose_gt[0][2]);
    state.v = Eigen::Vector2d(true_vx,true_vy);
    state.ba = Eigen::Vector2d(0,0);
    state.bg = init_bias;

    Optimizer optimizer;
    Eigen::Vector3d last_odometry = odometry_data[0];

	for(unsigned int i = 1; i < timestamps.size(); i++) {

		cout << " \ncurrent fusion idx is ==== : " << i << std::endl;

		const Eigen::Vector3d &last_imu = imu_data[i-1];
		const Eigen::Vector3d &curr_imu = imu_data[i];

		state.last_theta = state.theta;
		// imu state and cov update
		imu_integration.Integrate(true, last_imu, curr_imu, timestamps[i]-timestamps[i-1], state);

		Eigen::Vector3d delta_odometry = odometry_data[i] - last_odometry;

		if(delta_odometry.block<2,1>(0,0).norm() < 0.00001) {
			continue;
		}
		last_odometry = odometry_data[i];
		
		// cout << " pose_data[i-1] ==== : " << pose_data[i-1].transpose() << std::endl;
		cout << " pose_data[i] ==== : " << pose_data[i].transpose() << std::endl;
		cout << " vel_data[i] ==== : " << vel_data[i].transpose() << std::endl;

		// cout << " odometry_data[i-1] ==== : " << odometry_data[i-1].transpose() << std::endl;
		// cout << " odometry_data[i] ==== : " << odometry_data[i].transpose() << std::endl;
		
		double vx = vel_data[i][0];
        double vy = vel_data[i][1];
		const Eigen::Vector2d velocity(vx, vy);
		const Eigen::Vector3d &pose = pose_data[i];
		
		optimizer.OptimizeGN(state, velocity, pose);

		// std::cout << "velocity ============= : " << velocity.transpose() << std::endl;
		// std::cout << "pose ============= : " << pose.transpose() << std::endl;

		std::cout << "optimize state.theta ============= : " << state.theta << std::endl;
    	std::cout << "optimize state.p ============= : " << state.p.transpose() << std::endl;
    	std::cout << "optimize state.v ============= : " << state.v.transpose() << std::endl;
    	std::cout << "optimize state.ba ============= : " << state.ba.transpose() << std::endl;
    	std::cout << "optimize state.bg ============= : " << state.bg << std::endl;
    	std::cout << "optimize state.cov ============= : \n" << state.cov << std::endl;
    	
    	usleep(150000);
	}

	return 0;
}
